#include "AddForce3DHandlerSensorConfiguration.h"

using namespace MMM;


AddForce3DHandlerSensorConfiguration::AddForce3DHandlerSensorConfiguration() : AddAttachedSensorConfiguration<Force3DSensor>("Force3D", {"ForceX", "ForceY", "ForceZ"})
{
}

void AddForce3DHandlerSensorConfiguration::addSensorMeasurements(const std::vector<std::map<std::string, std::string> > &values) {
    for (std::map<std::string, std::string> map : values) {
        float timestep = XML::convertTo<float>(map[TIMESTEP_STR], "Timestep is no valid float!");
        Eigen::Vector3f force(XML::convertTo<float>(map["ForceX"], "Force in x-direction is no valid float!"),
                              XML::convertTo<float>(map["ForceY"], "Force in y-direction is no valid float!"),
                              XML::convertTo<float>(map["ForceZ"], "Force in z-direction is no valid float!"));
        Force3DSensorMeasurementPtr measurement(new Force3DSensorMeasurement(timestep, force));
        sensor->addSensorMeasurement(measurement);
    }
}

